#ifndef GAIT_H
#define GAIT_H

/* 宏 constrain 将输入限制在low high 之间 */
#define constrain(amt, low, high) \
  ((amt) < (low) ? (amt = low) : ((amt) > (high) ? (amt = high) : (amt = amt)))
/* 宏 limit 将输入限制在low high 范围内 */
#define limit(amt, low, high) \
  ((amt) < (low) ? (amt = high) : ((amt) > (high) ? (amt = low) : (amt = amt)))
/* 宏 PI 圆周率定义 */
#ifndef PI
#define PI 3.1415926f
#endif
//角度制转化为弧度制
#define ANGLE2RAD(x) (x / 180.0f * PI)
//弧度制转换为角度制
#define RAD2ANGLE(x) (x / PI * 180.0f)

#define L1 10 //大腿长度 单位 cm
#define L2 20 //小腿长度 单位 cm

typedef enum
{
  TROT = 0, //双排步态-对角小跑 左后+右前-右后+左前
  WALK = 1, //单拍步态-慢步 左后-左前-右后-右前
  GALLOP = 2, // 三拍步态-袭步 右后-左后+右前-左前

  STOP = 20,
  REALSE = 21,

  JUMP = 30

} GaitState_e;
extern GaitState_e GAIT_STATE;

typedef struct
{               // 腿部 PID增益 结构体
  float kp_pos; //位置环 kp
  float kd_pos; //位置环 kd
  float kp_spd; //速度环 kp
  float kd_spd; //速度环 kd
} LegGain_t;
extern LegGain_t gait_gains[];

typedef struct
{                       // 腿部参数结构体
  float stance_height;  // 狗身到地面的距离 (cm)
  float step_length;    // 一步的距离 (cm)
  float up_amp;         // 上部振幅y (cm)
  float down_amp;       // 下部振幅 (cm)
  float flight_percent; // 摆动相百分比 (cm)
  float freq;           // 一步的频率 (Hz)
} GaitParam_t;

typedef struct
{ // 腿部参数分离结构体
  GaitParam_t leg0;
  GaitParam_t leg1;
  GaitParam_t leg2;
  GaitParam_t leg3;
} GaitParams_t;
extern GaitParams_t gait_params[];

typedef struct
{ // 四足底盘结构体
  float leg0_offset;
  float leg1_offset;
  float leg2_offset;
  float leg3_offset;
  float leg0_direction;
  float leg1_direction;
  float leg2_direction;
  float leg3_direction;
} RobotBase_t;
extern RobotBase_t robot_base[];

extern float output_pos[8];

void GaitTask(void);

void gait(GaitParams_t params, LegGain_t gains, RobotBase_t robot_base);

void CoupledMoveLeg(float t, GaitParam_t param, float gait_offset, float leg_direction, int leg_id);

void SinTrajectory(float t, GaitParam_t param, float gait_offset, float *x, float *y);

void CartesianToThetaGamma(float x, float y, float leg_direction, float *theta, float *gamma);

void SetCoupledPosition(int leg_id, float theta, float gamma);

void CommandAllLegs(float theta, float gamma, LegGain_t gains);

// bool IsValidLegGain(LegGain gains);
// bool IsValidGaitParams(GaitParam param);
// void ChangeTheGainsOfPD(LegGain gains);
// void RenewYaw(void);
#endif
